#include <stdio.h>
#include "cmsis_os2.h"
#include "los_task.h"
#include "ohos_init.h"
#include <string.h>
#include "usart1.h" //usart1串口通信的部分
#include "../car_beep_demo/bsp_beep.h"
#include "protocol.h"
#include "../mpu9250_demo/bsp_iic_soft.h"
#include "../mpu9250_demo/bsp_mpu9250.h"
volatile uint8_t send_light_data = 0;
volatile uint8_t send_bme280_data = 0;
volatile uint8_t send_gas_data = 0;
volatile uint8_t send_pm_data = 0;
volatile uint8_t send_imu_data = 0;
volatile uint8_t send_gps_data = 0;

/********************************************************************************************************/
/* 解析上位机发送过来的命令, 并处理控制的内容 */
void vTask_Control(void)
{
    while (1)
    {
        if (Get_CMD_Flag())
        {
            Upper_Data_Parse(Get_RxBuffer(), Get_CMD_Length());
            Clear_CMD_Flag();
        }
        osDelay(1);
    }
}
/**
 * 创建任务
 * @param func 任务函数
 * @param name 任务名
 * @param stack_size 堆栈大小
 * @param priority 优先级
 * @param tid_out 任务ID
 */
static void VTask_Create(osThreadFunc_t func, const char *name, uint32_t stack_size, osPriority_t priority, osThreadId_t *tid_out)
{
    osThreadAttr_t attr;
    attr.name = name;
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = stack_size; // 堆栈大小调大一点
    attr.priority = priority;
    osThreadId_t tid = osThreadNew(func, NULL, &attr);
    if (tid_out)
        *tid_out = tid;
    if (tid == NULL)
    {
        // printf(" [%d] User_main_task_create create %s: NG\n", tid, name);
        printf(" [%p] User_main_task_create create %s: NG\n", (void*)tid, name);
    }
    else
    {
        // printf(" [%d]User_main_task_create create %s: OK\n", tid, name);
        printf(" [%p] User_main_task_create create %s: OK\n", (void*)tid, name);

    }
}
osThreadId_t trcae_tid;
void vTask_Trace(void *arg)
{
    printf("start trace\n");
    while (1)
    {
        timer_periodic();
        osDelay(20);
    }
}

void TaskSpeed(void *arg)
{
    // 10ms 测速线程
    uint32_t last_tick = osKernelGetTickCount();
    printf("TaskSpeed \n");
    while (1)
    {
        osDelay(10);
        // last_tick = osKernelGetTickCount();
        // osDelayUntil(last_tick + 10);
        Encoder_Update_Count();
        Motion_Handle();
    }
}
// void LIGHT_Sensor_TASK(void)
// {

//     uint16_t light_value = 0;
//     while (1)
//     {
//         light_value=light_get_data();
//         printf("light:%d%%\n", light_value);
//         osDelay(200);
//     }
// }
// float DATA1[3] = {0};
// void BME280_TASK(void)
// {
//     while (1)
//     {
//         Get_BME280_Value(DATA1);
//         printf("[BME280]Temp=");
//         printf("%0.3f,", DATA1[0]);
//         printf("[BME280]Hum=");
//         printf("%0.3f,", DATA1[1]);
//         printf("[BME280]Press=");
//         printf("%0.3f\r\n", DATA1[2]);
//         osDelay(200);
//     }
// }
// void GAS_Sensor_TASK(void)
// {

//     uint16_t gas_value = 0;
//     while (1)
//     {
//         gas_value=gas_get_data();
//         printf("gas_value：%d%%\n", gas_value);
//         osDelay(200);
//     }
// }
// void PM_Sensor_TASK(void)
// {

//     float pm25_value = 0;
//     while (1)
//     {
//         pm25_value=pm25_get_data();
//         printf("pm25_value:%.2f\n", pm25_value);   
//         osDelay(200);
//     }
// }
// extern mpu_data_t mpu_data;
// void vTask_IMU()
// {
//     uint32_t last_tick = osKernelGetTickCount();
//     while (1)
//     {
//         MPU9250_Read_Data_Handle();
//         printf("acc XYZ:|	%d	|	%d	|	%d	|\n", mpu_data.accel[0], mpu_data.accel[1], mpu_data.accel[2]);
//         printf("gyro XYZ:|	%d	|	%d	|	%d	|\n", mpu_data.gyro[0], mpu_data.gyro[1], mpu_data.gyro[2]);
//         printf("mag XYZ:|	%d	|	%d	|	%d	|\n", mpu_data.compass[0], mpu_data.compass[1], mpu_data.compass[2]);
//         last_tick = osKernelGetTickCount();
//         osDelayUntil(last_tick + 10); // 根据IMU采样频率
//     }
// }


void SendLightSensorData(uint8_t verify) {
    uint8_t response[8] = {0xFF, 0xFB, 0x06, 0x20, verify}; // 协议头：帧头+长度+指令类型+验证号
    uint16_t light_value = light_get_data();                // 获取最新光照数据
    cJSON *root = cJSON_CreateObject();
    if (root != NULL)
    {
        cJSON *serv_arr = cJSON_AddArrayToObject(root, "services");
        cJSON *arr_item = cJSON_CreateObject();
        cJSON_AddStringToObject(arr_item, "service_id", "icar");
        cJSON *pro_obj = cJSON_CreateObject();
        cJSON_AddItemToObject(arr_item, "properties", pro_obj);
        cJSON_AddNumberToObject(pro_obj, "light", light_value);
        cJSON_AddItemToArray(serv_arr, arr_item);
        // 生成 JSON 字符串
        char *payload_str = cJSON_PrintUnformatted(root);
        if (payload_str != NULL)
        {
            Serial3_SendString(payload_str);
            cJSON_free(payload_str);
        }
        // 删除 JSON 对象
        cJSON_Delete(root);
    }
    // 填充16位数据（低字节在前，小端模式）
    response[5] = light_value & 0xFF;         // 光照值低8位
    response[6] = (light_value >> 8) & 0xFF;  // 光照值高8位
    
    // 计算校验和（前11字节累加和）
    response[7] = CalculateChecksum(response, 7);
    
    // 打印并发送
    PrintPacket(response, 8);
    Serial1_SendArray(response, 8);
    printf("Sent light sensor data: %d%%\n", light_value);
}

// 2. BME280（温湿度气压）数据发送
void SendBME280Data(uint8_t verify) {
    uint8_t response[14] = {0xFF, 0xFB, 0x0C, 0x21, verify}; // 长度0x0C（12字节）
    float data[3];
    Get_BME280_Value(data); // 获取温度、湿度、气压数据
    
    // 浮点数放大1000倍转为32位整数（保留3位小数）
    int32_t temp = (int32_t)(data[0] * 1000);  // 温度
    int32_t hum  = (int32_t)(data[1] * 1000);  // 湿度
    int32_t press= (int32_t)(data[2] * 1000);  // 气压
    cJSON *root = cJSON_CreateObject();
    if (root != NULL)
    {
        cJSON *serv_arr = cJSON_AddArrayToObject(root, "services");
        cJSON *arr_item = cJSON_CreateObject();
        cJSON_AddStringToObject(arr_item, "service_id", "icar");
        cJSON *pro_obj = cJSON_CreateObject();
        cJSON_AddItemToObject(arr_item, "properties", pro_obj);
        cJSON_AddNumberToObject(pro_obj, "temp", temp);
        cJSON_AddNumberToObject(pro_obj, "hum", hum);
        cJSON_AddNumberToObject(pro_obj, "press", press);


        cJSON_AddItemToArray(serv_arr, arr_item);
        // 生成 JSON 字符串
        char *payload_str = cJSON_PrintUnformatted(root);
        if (payload_str != NULL)
        {
            Serial3_SendString(payload_str);
            cJSON_free(payload_str);
        }
        // 删除 JSON 对象
        cJSON_Delete(root);
    }
    // 填充数据（低字节在前）
    response[5]  = temp & 0xFF;
    response[6]  = (temp >> 8) & 0xFF;
    response[7]  = hum & 0xFF;
    response[8]  = (hum >> 8) & 0xFF;
    response[9]  = press & 0xFF;
    response[10] = (press >> 8) & 0xFF;
    
    // 校验和（前11字节）
    response[11] = CalculateChecksum(response, 11);
    
    PrintPacket(response, 12);
    Serial1_SendArray(response, 12);
    
    printf("Sent BME280 data: T=%.3f, H=%.3f, P=%.3f\n", data[0], data[1], data[2]);
}

// 3. 气体传感器数据发送
void SendGasSensorData(uint8_t verify) {
    uint8_t response[8] = {0xFF, 0xFB, 0x06, 0x22, verify}; // 指令类型0x22
    uint16_t gas_value = gas_get_data();                    // 获取气体浓度
    cJSON *root = cJSON_CreateObject();
    if (root != NULL)
    {
        cJSON *serv_arr = cJSON_AddArrayToObject(root, "services");
        cJSON *arr_item = cJSON_CreateObject();
        cJSON_AddStringToObject(arr_item, "service_id", "icar");
        cJSON *pro_obj = cJSON_CreateObject();
        cJSON_AddItemToObject(arr_item, "properties", pro_obj);
        cJSON_AddNumberToObject(pro_obj, "gas", gas_value);


        cJSON_AddItemToArray(serv_arr, arr_item);
        // 生成 JSON 字符串
        char *payload_str = cJSON_PrintUnformatted(root);
        if (payload_str != NULL)
        {
            Serial3_SendString(payload_str);
            cJSON_free(payload_str);
        }
        // 删除 JSON 对象
        cJSON_Delete(root);
    }
    // 填充16位数据
    response[5] = gas_value & 0xFF;
    response[6] = (gas_value >> 8) & 0xFF;
    
    response[7] = CalculateChecksum(response, 7);
    
    PrintPacket(response, 8);
    Serial1_SendArray(response, 8);
    printf("Sent gas sensor data: %d%%\n", gas_value);
}

// 4. PM2.5传感器数据发送
void SendPMSensorData(uint8_t verify) {
    uint8_t response[8] = {0xFF, 0xFB, 0x06, 0x23, verify}; // 指令类型0x23
    float pm25_value = pm25_get_data();                     // 获取PM2.5值
    cJSON *root = cJSON_CreateObject();
    if (root != NULL)
    {
        cJSON *serv_arr = cJSON_AddArrayToObject(root, "services");
        cJSON *arr_item = cJSON_CreateObject();
        cJSON_AddStringToObject(arr_item, "service_id", "icar");
        cJSON *pro_obj = cJSON_CreateObject();
        cJSON_AddItemToObject(arr_item, "properties", pro_obj);
        cJSON_AddNumberToObject(pro_obj, "pm2.5", pm25_value);
        


        cJSON_AddItemToArray(serv_arr, arr_item);
        // 生成 JSON 字符串
        char *payload_str = cJSON_PrintUnformatted(root);
        if (payload_str != NULL)
        {
            Serial3_SendString(payload_str);
            cJSON_free(payload_str);
        }
        // 删除 JSON 对象
        cJSON_Delete(root);
    }
    // 浮点数放大100倍转为16位整数（保留2位小数）
    int16_t pm_data = (int16_t)(pm25_value * 100);
    
    // 填充数据
    response[5] = pm_data & 0xFF;
    response[6] = (pm_data >> 8) & 0xFF;
    
    response[7] = CalculateChecksum(response, 7);
    
    PrintPacket(response, 8);
    Serial1_SendArray(response, 8);

    printf("Sent PM2.5 data: %.2f\n", pm25_value);
}

// 5. IMU（加速度+陀螺仪+磁力计）数据发送
extern mpu_data_t mpu_data; // 外部声明的IMU数据结构体
void SendIMUData(uint8_t verify) {
    uint8_t response[24] = {0xFF, 0xFB, 0x18, 0x24, verify}; // 长度0x18（24字节）
    MPU9250_Read_Data_Handle();  // 刷新最新数据
    int16_t accel_x = mpu_data.accel[0];
    int16_t accel_y = mpu_data.accel[1];
    int16_t accel_z = mpu_data.accel[2];
    int16_t gyro_x = mpu_data.gyro[0];
    int16_t gyro_y = mpu_data.gyro[1];
    int16_t gyro_z = mpu_data.gyro[2];
    int16_t mag_x = mpu_data.compass[0];
    int16_t mag_y = mpu_data.compass[1];
    int16_t mag_z = mpu_data.compass[2];

    // 2. 构建JSON
    cJSON *root = cJSON_CreateObject();
    if (root == NULL) return;

    // 外层services数组
    cJSON *serv_arr = cJSON_AddArrayToObject(root, "services");
    // 单个service对象
    cJSON *arr_item = cJSON_CreateObject();
    cJSON_AddStringToObject(arr_item, "service_id", "icar");  // 固定service_id

    // properties对象（存放具体变量）
    cJSON *pro_obj = cJSON_CreateObject();
    cJSON_AddNumberToObject(pro_obj, "accel_x", accel_x);
    cJSON_AddNumberToObject(pro_obj, "accel_y", accel_y);
    cJSON_AddNumberToObject(pro_obj, "accel_z", accel_z);
    cJSON_AddNumberToObject(pro_obj, "gyro_x", gyro_x);
    cJSON_AddNumberToObject(pro_obj, "gyro_y", gyro_y);
    cJSON_AddNumberToObject(pro_obj, "gyro_z", gyro_z);
    cJSON_AddNumberToObject(pro_obj, "mag_x", mag_x);
    cJSON_AddNumberToObject(pro_obj, "mag_y", mag_y);
    cJSON_AddNumberToObject(pro_obj, "mag_z", mag_z);

    // 3. 组装JSON结构
    cJSON_AddItemToObject(arr_item, "properties", pro_obj);
    cJSON_AddItemToArray(serv_arr, arr_item);

    // 4. 生成JSON字符串并发送
    char *payload_str = cJSON_PrintUnformatted(root);
    if (payload_str != NULL) {
        Serial3_SendString(payload_str);  // 通过Serial3发送
        printf("IMU JSON sent: %s\n", payload_str);
        cJSON_free(payload_str);  // 释放内存
    }

    // 5. 清理资源
    cJSON_Delete(root);    
    // 填充加速度数据（3轴，各16位）
    response[5]  = mpu_data.accel[0] & 0xFF;
    response[6]  = (mpu_data.accel[0] >> 8) & 0xFF;
    response[7]  = mpu_data.accel[1] & 0xFF;
    response[8]  = (mpu_data.accel[1] >> 8) & 0xFF;
    response[9]  = mpu_data.accel[2] & 0xFF;
    response[10] = (mpu_data.accel[2] >> 8) & 0xFF;
    
    // 填充陀螺仪数据（3轴，各16位）
    response[11] = mpu_data.gyro[0] & 0xFF;
    response[12] = (mpu_data.gyro[0] >> 8) & 0xFF;
    response[13] = mpu_data.gyro[1] & 0xFF;
    response[14] = (mpu_data.gyro[1] >> 8) & 0xFF;
    response[15] = mpu_data.gyro[2] & 0xFF;
    response[16] = (mpu_data.gyro[2] >> 8) & 0xFF;
    
    // 填充磁力计数据（3轴，各16位）
    response[17] = mpu_data.compass[0] & 0xFF;
    response[18] = (mpu_data.compass[0] >> 8) & 0xFF;
    response[19] = mpu_data.compass[1] & 0xFF;
    response[20] = (mpu_data.compass[1] >> 8) & 0xFF;
    response[21] = mpu_data.compass[2] & 0xFF;
    response[22] = (mpu_data.compass[2] >> 8) & 0xFF;
    
    // 校验和（前23字节）
    response[23] = CalculateChecksum(response, 23);
    
    PrintPacket(response, 24);
    Serial1_SendArray(response, 24);

    printf("Sent IMU data: acc=[%d,%d,%d], gyro=[%d,%d,%d], mag=[%d,%d,%d]\n",
           mpu_data.accel[0], mpu_data.accel[1], mpu_data.accel[2],
           mpu_data.gyro[0], mpu_data.gyro[1], mpu_data.gyro[2],
           mpu_data.compass[0], mpu_data.compass[1], mpu_data.compass[2]);
}
void SendGPSLocationData(uint8_t verify) {
    uint8_t response[16] = {0xFF, 0xFB, 0x06, 0x25, verify}; // 指令类型0x24（假设为GPS位置数据）
    
    // 从全局变量中获取GPS数据
    float latitude = atof(Save_Data.latitude);  // 转换纬度为浮点数
    float longitude = atof(Save_Data.longitude); // 转换经度为浮点数
    char ns = Save_Data.N_S[0];                  // 南北半球（'N'/'S'）
    char ew = Save_Data.E_W[0];                  // 东西半球（'E'/'W'）

    // 2. 构建JSON
    cJSON *root = cJSON_CreateObject();
    if (root == NULL) return;

    // 外层services数组
    cJSON *serv_arr = cJSON_AddArrayToObject(root, "services");
    // 单个service对象
    cJSON *arr_item = cJSON_CreateObject();
    cJSON_AddStringToObject(arr_item, "service_id", "icar");

    // properties对象
    cJSON *pro_obj = cJSON_CreateObject();
    cJSON_AddNumberToObject(pro_obj, "latitude", latitude);   // 纬度值
    cJSON_AddStringToObject(pro_obj, "ns", &ns);              // 南北半球
    cJSON_AddNumberToObject(pro_obj, "longitude", longitude); // 经度值
    cJSON_AddStringToObject(pro_obj, "ew", &ew);              // 东西半球

    // 3. 组装JSON结构
    cJSON_AddItemToObject(arr_item, "properties", pro_obj);
    cJSON_AddItemToArray(serv_arr, arr_item);

    // 4. 生成JSON字符串并发送
    char *payload_str = cJSON_PrintUnformatted(root);
    if (payload_str != NULL) {
        Serial3_SendString(payload_str);
        printf("GPS JSON sent: %s\n", payload_str);
        cJSON_free(payload_str);
    }

    // 5. 清理资源
    cJSON_Delete(root);

    // 浮点数放大100000倍转为32位整数（保留5位小数）
    int32_t lat_data = (int32_t)(latitude * 100000);
    int32_t lon_data = (int32_t)(longitude * 100000);
    
    // 填充纬度数据（低字节在前）
    response[5] = lat_data & 0xFF;
    response[6] = (lat_data >> 8) & 0xFF;
    response[7] = (lat_data >> 16) & 0xFF;
    response[8] = (lat_data >> 24) & 0xFF;
    
    // 填充经度数据
    response[9] = lon_data & 0xFF;
    response[10] = (lon_data >> 8) & 0xFF;
    response[11] = (lon_data >> 16) & 0xFF;
    response[12] = (lon_data >> 24) & 0xFF;
    
    // 添加南北半球和东西半球标识
    response[13] = Save_Data.N_S[0]; // 'N' 或 'S'
    response[14] = Save_Data.E_W[0]; // 'E' 或 'W'
    
    // 计算校验和
    response[15] = CalculateChecksum(response, 15);
    
    // 发送数据包
    PrintPacket(response, 16);
    Serial1_SendArray(response, 16);

    // 打印发送信息
    printf("Sent GPS data: Lat=%.5f %c, Lon=%.5f %c\n", 
           latitude, Save_Data.N_S[0], 
           longitude, Save_Data.E_W[0]);
}
void LIGHT_Sensor_TASK()
{
    uint16_t light_value = 0;
            SendLightSensorData(0xAA); // 假设验证号为0xAA

        light_value = light_get_data();
        // printf("light:%d%%\n", light_value);
        
        // 新增：当标志被设置时发送数据到上位机
        if (send_light_data) {
            send_light_data = 0;       // 清除发送标志
        }
        
        osDelay(200);
    
}

float DATA1[3] = {0};
void BME280_TASK()
{
            SendBME280Data(0xAA); // 假设验证号为0xAA

        // Get_BME280_Value(DATA1);
        // printf("[BME280]Temp=");
        // printf("%0.3f,", DATA1[0]);
        // printf("[BME280]Hum=");
        // printf("%0.3f,", DATA1[1]);
        // printf("[BME280]Press=");
        // printf("%0.3f\r\n", DATA1[2]);
        
        // 新增：当标志被设置时发送数据到上位机
        if (send_bme280_data) {
            send_bme280_data = 0;  // 清除发送标志
        }
        
        osDelay(200);
    
}

void GAS_Sensor_TASK()
{
    uint16_t gas_value = 0;
            SendGasSensorData(0xAA); // 修正：调用GPS发送函数

        // gas_value = gas_get_data();
        // printf("gas_value：%d%%\n", gas_value);
        
        // 新增：当标志被设置时发送数据到上位机
        if (send_gas_data) {
            send_gas_data = 0;       // 清除发送标志
        }
        
        osDelay(200);
    
}

void PM_Sensor_TASK()
{
    float pm25_value = 0;

                SendPMSensorData(0xAA); // 假设验证号为0xAA

        // pm25_value = pm25_get_data();
        // printf("pm25_value:%.2f\n", pm25_value);   
        
        // 新增：当标志被设置时发送数据到上位机
        if (send_pm_data) {
            send_pm_data = 0;       // 清除发送标志
        }
        
        osDelay(200);
    
}

extern mpu_data_t mpu_data;
void vTask_IMU()
{
    uint32_t last_tick = osKernelGetTickCount();
            SendIMUData(0xAA); // 假设验证号为0xAA

        // MPU9250_Read_Data_Handle();
        // printf("acc XYZ:|    %d    |    %d    |    %d    |\n", mpu_data.accel[0], mpu_data.accel[1], mpu_data.accel[2]);
        // printf("gyro XYZ:|    %d    |    %d    |    %d    |\n", mpu_data.gyro[0], mpu_data.gyro[1], mpu_data.gyro[2]);
        // printf("mag XYZ:|    %d    |    %d    |    %d    |\n", mpu_data.compass[0], mpu_data.compass[1], mpu_data.compass[2]);
        
        // 新增：当标志被设置时发送数据到上位机
        if (send_imu_data) {
            send_imu_data = 0; // 清除发送标志
        }
        
        last_tick = osKernelGetTickCount();
        osDelayUntil(last_tick + 10); // 根据IMU采样频率
    
}

void GPS_TASK()
{

            SendGPSLocationData(0xAA); // 假设验证号为0xAA

		// parseGpsBuffer();
		// printGpsBuffer();
        if (send_gps_data) {
            send_gps_data = 0; // 清除发送标志
        }
	
}

// 其他SendXXXData函数保持不变...

/** 新增：定时触发发送任务 **/
void SensorSendTask(void *arg) {
    static uint32_t send_counter = 0;
        while (1)
        {
            /* code */
        
        
        // 每500ms触发一次发送（根据需要调整频率）
            osDelay(500);
            
            // 使用计数器轮流发送不同传感器数据
            send_counter++;
            
            switch (send_counter % 6) {
                case 0:
                    send_light_data = 1;
                    LIGHT_Sensor_TASK();
                    break;
                case 1:
                    send_bme280_data = 1;
                    BME280_TASK();
                    break;
                case 2:
                    send_gas_data = 1;
                    GAS_Sensor_TASK();
                    break;
                case 3:
                    send_pm_data = 1;
                    PM_Sensor_TASK();
                    break;
                case 4:
                    send_imu_data = 1;
                    vTask_IMU();
                    break;
                case 5:
                    send_gps_data = 1;
                    GPS_TASK();
                    break;
        }
        
    }
}

void TaskLight(void)
{
    printf("TaskLight started\n");
    
    while (1)
    {
        osDelay(50);  // 50ms任务周期（可根据需要调整）
        
        // 处理双闪灯逻辑
        if (light_hazard_enabled) {
            uint32_t current_tick = osKernelGetTickCount();
            
            // 每500ms切换一次灯光状态（闪烁频率）
            if (current_tick - light_last_tick >= 500) {
                light_hazard_toggle = !light_hazard_toggle;
                light_last_tick = current_tick;
                
                if (light_hazard_toggle) {
                    set_on_left_light();
                    set_off_right_light();
                } else {
                    set_off_left_light();
                    set_on_right_light();
                }
            }
        }
    }
}



void User_main_task_create(void)
{
    /****************************************************/
    ctrl_uart1_init();
    bsp_uart3_init(115200);
    uart7_init(115200);
	clrStruct();
    /****************************************************/
    // 初始化板载蜂鸣器
    beep_init();
    motor_init();
    gas_init();
    BME280_Init();
    encoder_init();
     // pid 参数初始化
    PID_Param_Init();
    //循迹模块初始化
    Trace_init();
    MPU9250_Init();
    car_light_init();
    /****************************************************/
    // VTask_Create(vTask_Control, "vTask_Control", 1024 * 20, osPriorityLow + 7,NULL);
    // VTask_Create(vTask_Trace, "vTask_Trace", 1024 * 20, osPriorityLow+2, &trcae_tid);
    // VTask_Create(TaskSpeed, "TaskSpeed", 1024 * 20, osPriorityLow + 8,NULL); 
    // VTask_Create(vTask_IMU, "vTask_IMU", 1024 * 20, osPriorityLow, NULL);
    // VTask_Create(TaskLight, "TaskLight", 1024 * 20, osPriorityLow+1, NULL); 
    // VTask_Create(BME280_TASK, "BME280_TASK", 1024 * 20, osPriorityLow, NULL);
    // VTask_Create(GAS_Sensor_TASK, "GAS_Sensor_TASK", 1024 * 20, osPriorityLow, NULL);
    // VTask_Create(LIGHT_Sensor_TASK, "LIGHT_Sensor_TASK", 1024 * 20, osPriorityLow, NULL);
    // VTask_Create(PM_Sensor_TASK, "PM_Sensor_TASK", 1024 * 20, osPriorityLow, NULL);

    // VTask_Create(SensorSendTask, "SensorSendTask", 1024 * 20, osPriorityLow, NULL);
    VTask_Create(vTask_Control, "vTask_Control", 1024 * 20, osPriorityNormal+2, NULL);
    
    // 高速循环任务（数据处理）
    VTask_Create(TaskSpeed, "TaskSpeed", 1024 * 20, osPriorityNormal + 8, NULL);
    // VTask_Create(vTask_IMU, "vTask_IMU", 1024 * 4, osPriorityNormal - 1, NULL);
    
    // 中等优先级任务
    VTask_Create(vTask_Trace, "vTask_Trace", 1024 * 20, osPriorityLow + 5, &trcae_tid);
    VTask_Create(TaskLight, "TaskLight", 1024 * 2, osPriorityLow + 1, NULL);
    
    // 传感器数据采集任务（不需要大栈空间）
    // VTask_Create(BME280_TASK, "BME280_TASK", 1024 * 4, osPriorityLow, NULL);
    // VTask_Create(GAS_Sensor_TASK, "GAS_Sensor_TASK", 1024 *4, osPriorityLow, NULL);
    // VTask_Create(LIGHT_Sensor_TASK, "LIGHT_Sensor_TASK", 1024 * 4, osPriorityLow, NULL);
    // VTask_Create(PM_Sensor_TASK, "PM_Sensor_TASK", 1024 * 4, osPriorityLow, NULL);
    // VTask_Create(GPS_TASK, "GPS_TASK", 1024 * 4, osPriorityLow, NULL);

    // 数据发送任务（周期性执行，栈需求小）
    VTask_Create(SensorSendTask, "SensorSendTask", 1024 * 20, osPriorityLow, NULL);
    // 新增：默认关闭循迹（等待指令开启）
    Trace_Ctrl(0);  // 初始化为关闭状态
}
APP_FEATURE_INIT(User_main_task_create);
